#include <stdio.h>
#include <windows.h>

#include <math.h>


// 电机参数
const float P = 5; // 极对数
const float lambda_m = 0.0167; // 永磁体磁链
const float Ld = 0.000035; // d轴电感
const float Lq = 0.00005; // q轴电感



// 控制参数
const float Kp_id = 1.0; // d轴电流PI控制器比例增益
const float Ki_id = 0.01; // d轴电流PI控制器积分增益
const float Kp_iq = 1.0; // q轴电流PI控制器比例增益
const float Ki_iq = 0.01; // q轴电流PI控制器积分增益

// 电流限制
const float Imax = 700.0; // 最大电流限制



// void CalculateMTPACurrents(float Te, float *id_ref, float *iq_ref) {
//     float k_m = 1.5 * P * lambda_m;
//     float k_ldq = 1.5 * P * (Ld - Lq);

//     // 初始猜测
//     float id = 0;
//     float step = 0.001;

//     // 使用牛顿-拉夫逊法求解i_d
//     for (int i = 0; i < 1000; i++) {
//         float f = id * pow((k_m + k_ldq * id), 3) - Te * Te * k_ldq;
//         float df = pow((k_m + k_ldq * id), 3) + 3 * k_ldq * id * pow((k_m + k_ldq * id), 2);
//         id = id - f / df;
//         if (fabs(f) < 1e-6) {
//             break;
//         }
//     }

//     *id_ref = id;
//     *iq_ref = Te / (k_m + k_ldq * id);
// }




//数值求解
void CalculateMTPACurrents(float Te, float *id_ref, float *iq_ref) {
    float k_m = 1.5 * P * lambda_m;
    float k_ldq = 1.5 * P * (Ld - Lq);

    // 初始猜测
    float id = 0.0;
    float id_prev = 0.0;
    float tolerance = 1e-6;
    int max_iter = 1000;

    // 使用牛顿-拉夫逊法求解i_d
    for (int i = 0; i < max_iter; i++) {
        float f = id * pow((k_m + k_ldq * id), 3) - Te * Te * k_ldq;
        float df = pow((k_m + k_ldq * id), 3) + 3 * k_ldq * id * pow((k_m + k_ldq * id), 2);
        id_prev = id;
        id = id - f / df;
        if (fabs(id - id_prev) < tolerance) {
            break;
        }
    }

    *id_ref = id;
    *iq_ref = Te / (k_m + k_ldq * id);

    // 电流限制
    if (fabs(*id_ref) > Imax) {
        *id_ref = (*id_ref > 0) ? Imax : -Imax;
    }
    if (fabs(*iq_ref) > Imax) {
        *iq_ref = (*iq_ref > 0) ? Imax : -Imax;
    }
}



//几何解析法
// void CalculateMTPACurrents1(float Te, float *id_ref, float *iq_ref) {
//     float k_m = 1.5 * P * lambda_m;
//     float k_ldq = 1.5 * P * (Ld - Lq);

//     // 计算i_d参考值
//     float id = sqrt((Te * Te - k_m * k_m) / (k_ldq * k_ldq));

//     // 计算i_q参考值
//     float iq = Te / (k_m + k_ldq * id);

//     *id_ref = id;
//     *iq_ref = iq;
// }

//使用这个     方程解析法
void CalculateMTPACurrents1(float Te, float *id_ref, float *iq_ref) {
    float id = -lambda_m / (2 * (Ld - Lq));
    float iq = sqrt(pow((2 * Te) / (3 * P * (Ld - Lq)), 2) - pow(id, 2));
    *id_ref = id;
    *iq_ref = iq;
}

typedef struct {
    float id;
    float iq;
} MTPA_Table_Entry;

MTPA_Table_Entry mtpa_table[440];



int main()
{

    int i=0;
    for(i;i<441;i++)
    {
       // 目标转矩
      float torque_ref = -i/10.0;

    // 使用MTPA策略计算d轴和q轴电流参考值

     float id_ref, iq_ref,id_ref1,iq_ref1;



      CalculateMTPACurrents(torque_ref, &id_ref, &iq_ref);

      //CalculateMTPACurrents1(torque_ref, &id_ref1, &iq_ref1);




    // 输出参考值
    //printf("torque_ref:%.2f,id_ref: %f, iq_ref: %f\n",torque_ref, id_ref, iq_ref);

    printf(" mtpa_table[%d].iq=%ff;\n",i,iq_ref);
    printf(" mtpa_table[%d].id=%ff;\n",i,id_ref);
    printf("\n");




    //printf("id_ref1: %f, iq_ref1: %f\n\n\n", id_ref1, iq_ref1);
    

    }

       






    system("pause");
    return 0;
}